﻿#include <Define.h>
#include <PhysicsWorldFunctions.h>
#include <memory>
#include <btBulletDynamicsCommon.h>
#include <osgbDynamics/MotionState.h>
#include <osgbDynamics/CreationRecord.h>
#include <osgbDynamics/RigidBody.h>
#include <osgbCollision/Utils.h>
#include <osgbCollision/GLDebugDrawer.h>
#include <SceneViewer.h>
#include <SceneViewerFunctions.h>
#include <osgDB/WriteFile>
#include <PhysicsWorld.h>
#include <TransformMatrixVisitor.h>
#include <CreatRigidBodyForMTVisitor.h>
//#include <ContactTestCallBack.h>
//#include <OpenTissueFunction.h>
#include <string>
#include <sstream>
#include <QtXml/QDomElement>
#include <QtCore/QFile>
#include <QtCore/QTextStream>
#include <JlCompress.h>
#include <SDFManager.h>
#include <Point3D.h>
using namespace PhysicsWorldFunctions;



osg::Vec3 PhysicsWorldFunctions::btVector3ToOsgVec3( const btVector3& bt_vec3)
{
    double x = bt_vec3[0];
    double y = bt_vec3[1];
    double z = bt_vec3[2];

    osg::Vec3 osg_vec3(x,y,z);
    return osg_vec3;
}


btRigidBody* PhysicsWorldFunctions::creatRigidBodyFromOsgNode(osg::ref_ptr<osg::Node> osg_node, double mass, COLLISION_SHAPE collision_shape_enum)
{
    osg::ref_ptr<osg::Transform> transform_ptr = dynamic_cast<osg::Transform*>(osg_node.get());
    if (transform_ptr == NULL)
    {
        return NULL;
    }
    TransformMatrixVisitor trans_matrix_visitor;
    transform_ptr->accept(trans_matrix_visitor);

    if (collision_shape_enum == BOX_SHAPE_PROXYTYPE || collision_shape_enum == SPHERE_SHAPE_PROXYTYPE)
    {
        osg::ref_ptr<osg::MatrixTransform> mt = dynamic_cast<osg::MatrixTransform*>(osg_node.get());
        //std::cout<<"PhysicsWorldFunctions::creatRigidBodyFromOsgNode"<<mt->getMatrix().getTrans().z()<<std::endl;

        osg::Matrix m = trans_matrix_visitor.getLocalMatrix();
        std::cout<<"PhysicsWorldFunctions::creatRigidBodyFromOsgNode"<<m.getTrans().z()<<std::endl;

        osgbDynamics::MotionState * motion = new osgbDynamics::MotionState;
        motion->setTransform(transform_ptr.get());
        motion->setParentTransform(osg::Matrix::identity());

        btCompoundShape* compound_shape = osgbCollision::btCompoundShapeFromBounds(transform_ptr, (BroadphaseNativeTypes)collision_shape_enum);


        btVector3 inertia;
        compound_shape->calculateLocalInertia(mass,inertia);

        btRigidBody::btRigidBodyConstructionInfo rbinfo( mass, motion, compound_shape, inertia );

        btRigidBody* rigidBody = new btRigidBody( rbinfo );
        //std::shared_ptr<btRigidBody> rigid_body_ptr(rigidBody);
        //std::string id_string = SceneViewerFunctions::osgObjectGetDescription(transform_ptr, "id");

        int int_id = 0;//LuaFunctions::stdStringToInt(id_string)
        //PhysicsWorld::instance()->addRigidBody(id_string, rigid_body_ptr);
        rigidBody->setUserIndex(int_id);
        PhysicsWorld::instance()->addRigidBody(rigidBody);
        rigidBody->setWorldTransform(osgbCollision::asBtTransform(m));

        trans_matrix_visitor.makeSetMode();
        trans_matrix_visitor.setLocalMatrix(m);
        std::cout<<"PhysicsWorldFunctions::creatRigidBodyFromOsgNode"<<mt->getMatrix().getTrans().z()<<std::endl;

        return rigidBody;
    }
    else if (collision_shape_enum == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
    {
        /*  osgBullet code */
        osg::Matrix m = trans_matrix_visitor.getLocalMatrix();
        osgbDynamics::MotionState * motion = new osgbDynamics::MotionState;
        motion->setTransform(transform_ptr.get());
        motion->setParentTransform(osg::Matrix::identity());


        btCollisionShape * collision = osgbCollision::btConvexTriMeshCollisionShapeFromOSG( transform_ptr.get() );

        /*  BULLET CODE */
        btTransform bodyTransform;
        bodyTransform.setIdentity();
        //bodyTransform.setOrigin( btVector3( 0, 0, 5 ) );
        //motion->setWorldTransform( bodyTransform );


        btVector3 inertia;
        collision->calculateLocalInertia( mass, inertia );
        btRigidBody::btRigidBodyConstructionInfo rbinfo( mass, motion, collision, inertia );
        btRigidBody * rigidBody = new btRigidBody( rbinfo );
        //body->setLinearVelocity( btVector3( -5, -1, 0 ) );
        //body->setAngularVelocity( btVector3( 1, 0, 0 ) );
        //std::shared_ptr<btRigidBody> rigid_body_ptr(rigidBody);

        //std::string id_string = SceneViewerFunctions::osgObjectGetDescription(transform_ptr, "id");

        int int_id = 0;//LuaFunctions::stdStringToInt(id_string)
        rigidBody->setUserIndex(int_id);
        PhysicsWorld::instance()->addRigidBody(rigidBody);

        rigidBody->setWorldTransform(osgbCollision::asBtTransform(m));

        trans_matrix_visitor.makeSetMode();
        trans_matrix_visitor.setLocalMatrix(m);


        return rigidBody;
    }
    else if (collision_shape_enum == TRIANGLE_MESH_SHAPE_PROXYTYPE)
    {
        /*  osgBullet code */
        osg::Matrix m = trans_matrix_visitor.getLocalMatrix();
        osgbDynamics::MotionState * motion = new osgbDynamics::MotionState;
        motion->setTransform(transform_ptr.get());
        motion->setParentTransform(osg::Matrix::identity());


        btCollisionShape * collision = osgbCollision::btTriMeshCollisionShapeFromOSG( transform_ptr.get() );

        /*  BULLET CODE */
        btTransform bodyTransform;
        bodyTransform.setIdentity();
        //bodyTransform.setOrigin( btVector3( 0, 0, 5 ) );
        //motion->setWorldTransform( bodyTransform );


        btVector3 inertia;
        //collision->calculateLocalInertia( mass, inertia );
        btRigidBody::btRigidBodyConstructionInfo rbinfo( mass, motion, collision, inertia );
        btRigidBody * rigidBody = new btRigidBody( rbinfo );
        //body->setLinearVelocity( btVector3( -5, -1, 0 ) );
        //body->setAngularVelocity( btVector3( 1, 0, 0 ) );

        //std::string id_string = SceneViewerFunctions::osgObjectGetDescription(transform_ptr, "id");

        int int_id = 0;//LuaFunctions::stdStringToInt(id_string)
        rigidBody->setUserIndex(int_id);
        PhysicsWorld::instance()->addRigidBody(rigidBody);


        rigidBody->setWorldTransform(osgbCollision::asBtTransform(m));

        trans_matrix_visitor.makeSetMode();
        trans_matrix_visitor.setLocalMatrix(m);

        //openTissue's code
        //writeTransformToObjFileForOpenTissue(rigidBody);
        //OpenTissueFunction::pushCommandConvert("tempFiles/AutomGenerate.obj", rigidBody);
        //OpenTissueFunction::convertCollisionObjToMeshType("tempFiles/AutomGenerate.obj", rigidBody);
        if(osg_node->getName() == "VICE JAW.1")
        {
            //std::cout<<"PhysicsWorldFunctions::creatRigidBodyFromOsgNode nodeName="<<osg_node->getName()<<std::endl;
            //rigidBody->setUserIndex((int)1);
            //std::cout<<"PhysicsWorldFunctions::creatRigidBodyFromOsgNode rigidBody="<<rigidBody->getUserIndex()<<std::endl;
            //SDFManager::instance()->addPtrActualToptr((int)rigidBody, 153356400);

        }
        else if(osg_node->getName() == "JAW SCREW.1")
        {
            //rigidBody->setUserIndex(2);
            //SDFManager::instance()->addPtrActualToptr((int)rigidBody, 153357968);

        }

        return rigidBody;
    }


    return NULL;
}

void PhysicsWorldFunctions::creatRigidGround()
{

}

void PhysicsWorldFunctions::creatRigidBodyForChildNode(osg::ref_ptr<osg::Node> osg_node)
{
    if (osg_node != NULL)
    {
        CreatRigidBodyForMTVisitor visitor;
        osg_node->accept(visitor);
    }

}
osg::Node * PhysicsWorldFunctions::rigidBodyToOsgNode(btRigidBody *rigidPtr)
{
    osg::Node* nodePtr = NULL;
    if(rigidPtr != NULL)
    {
        osgbDynamics::MotionState* motion = dynamic_cast<osgbDynamics::MotionState*>(rigidPtr->getMotionState());
        if(motion != NULL)
        {
            nodePtr = motion->getTransform();
        }
    }
    return nodePtr;
}
btRigidBody* PhysicsWorldFunctions::findRigidBodyByNodeName(std::string nodeName)
{
    btRigidBody* rigidPtr = NULL;
    btCollisionObjectArray objArray = PhysicsWorld::instance()->getDynamicsWorld()->getCollisionObjectArray();
    for(int i=0 ; i<objArray.size(); i++)
    {
        btCollisionObject* objPtr = objArray.at(i);
        osg::Node * nodePtr = rigidBodyToOsgNode( dynamic_cast<btRigidBody*>(objPtr) );
        if(nodePtr != NULL)
        {
            if(nodePtr->getName() == nodeName)
            {
                rigidPtr = dynamic_cast<btRigidBody*>(objPtr);
                break;
            }
        }
    }
    return rigidPtr;
}

std::vector<osg::Vec3> PhysicsWorldFunctions::rigidBodyObbAxis(btRigidBody *rigidPtr)
{
    std::vector<osg::Vec3> axis;
    osg::Node* nodePtr = rigidBodyToOsgNode(rigidPtr);
    if(nodePtr != NULL)
    {
        SceneViewerFunctions::computeObbIgnoreSubSubChildNode(nodePtr);
    }
    return axis;
}

void PhysicsWorldFunctions::enablePhysicsDebugDraw()
{
    SceneViewer::instance()->addChild( PhysicsWorld::instance()->getDebugDraw()->getSceneGraph() );

    PhysicsWorld::instance()->setEnableDebugDraw(true);
}

void PhysicsWorldFunctions::disablePhysicsDebugDraw()
{
    SceneViewer::instance()->removeChild( PhysicsWorld::instance()->getDebugDraw()->getSceneGraph() );

    PhysicsWorld::instance()->setEnableDebugDraw(false);
}
bool PhysicsWorldFunctions::getEnableDebugDraw()
{
    return PhysicsWorld::instance()->getEnableDebugDraw();
}

void PhysicsWorldFunctions::setGravity( double x, double y, double z )
{
    PhysicsWorld::instance()->getDynamicsWorld()->setGravity(btVector3( x, y, z ));
}

osg::Vec3 PhysicsWorldFunctions::getGravity()
{

    double x = PhysicsWorld::instance()->getDynamicsWorld()->getGravity()[0];
    double y = PhysicsWorld::instance()->getDynamicsWorld()->getGravity()[1];
    double z = PhysicsWorld::instance()->getDynamicsWorld()->getGravity()[2];

    osg::Vec3 g(x,y,z);
    return g;
}

void PhysicsWorldFunctions::enableDynamic()
{

    PhysicsWorld::instance()->setEnableDynamic(true);
}

void PhysicsWorldFunctions::disableDynamic()
{

    PhysicsWorld::instance()->setEnableDynamic(false);
}

// bool PhysicsWorldFunctions::contactTestByNameAndMatrix( const std::string& model_name, const osg::Matrix& m )
// {
// 	std::shared_ptr<btRigidBody> rigid_body_ptr = PhysicsWorld::instance()->getRigidBody(model_name);
// 	//如果没有对应的刚体，就直接返回true
// 	if (rigid_body_ptr.get() == NULL)
// 	{
// 		return false;
// 	}
// 
// 	
// 	PhysicsWorld::instance()->getDynamicsWorld()->performDiscreteCollisionDetection();
// 	rigid_body_ptr->setWorldTransform(osgbCollision::asBtTransform(m));
// 	SceneViewerFunctions::setModelBaseMatrix(model_name, m);
// 
// 
// 	ContactTestCallBack contact_test_call_back;
// 	PhysicsWorld::instance()->getDynamicsWorld()->contactTest(rigid_body_ptr.get(), contact_test_call_back);
// 	btManifoldPoint manifold_point = contact_test_call_back.getManifoldPoint();
// 	double dis = manifold_point.getDistance();
// 	if (dis < -1000000 || dis > 1000000)
// 	{
// 		return false;
// 	}
// 	return true;
// }




btRigidBody* PhysicsWorldFunctions::physicsWorldGetRigidBody( int int_id )
{
    return PhysicsWorld::instance()->getRigidBody(int_id);
}

bool PhysicsWorldFunctions::rigidBodyIsAvailable(int int_id)
{
    bool bool_value = false;
    btRigidBody* rigid_body = PhysicsWorld::instance()->getRigidBody(int_id);
    if (rigid_body != NULL)
    {
        bool_value = true;
    }
    return bool_value;
}


void PhysicsWorldFunctions::rigidBodyApplyCentralForce( btRigidBody* body_ptr, double x, double y, double z )
{
    if (body_ptr != NULL)
    {
        body_ptr->applyCentralForce(btVector3( x, y, z ));
    }

}

osg::Vec3 PhysicsWorldFunctions::rigidBodyGetLinearVelocity( btRigidBody* body_ptr )
{
    btVector3 force(1,0,0);

    if (body_ptr != NULL)
    {
        //force =  body_ptr->getTotalForce();
        force = body_ptr->getLinearVelocity();
        //std::cout<<force[0]<<std::endl;
    }


    return btVector3ToOsgVec3(force);
}

void PhysicsWorldFunctions::contactTestByIdStr( int int_id )
{
    btRigidBody* rigid_body_ptr = PhysicsWorld::instance()->getRigidBody(int_id);
    //如果没有对应的刚体，就直接返回true
    if (rigid_body_ptr == NULL)
    {
        return;
    }
    PhysicsWorld::instance()->getDynamicsWorld()->performDiscreteCollisionDetection();
    ContactTestCallBack contact_test_call_back;
    PhysicsWorld::instance()->getDynamicsWorld()->contactTest(rigid_body_ptr, contact_test_call_back);

}


int PhysicsWorldFunctions::contactTestCallBackGetManifoldVectorSize(int int_id)
{
    ContactTestCallBack contact_test_call_back;
    btRigidBody* rigid_body_ptr = PhysicsWorld::instance()->getRigidBody(int_id);
    //如果没有对应的刚体，就直接返回true
    if (rigid_body_ptr != NULL)
    {
        PhysicsWorld::instance()->getDynamicsWorld()->performDiscreteCollisionDetection();

        PhysicsWorld::instance()->getDynamicsWorld()->contactTest(rigid_body_ptr, contact_test_call_back);

    }
    return contact_test_call_back.getManifoldVectorSize();
}


osg::Vec3 PhysicsWorldFunctions::computeSeparateDirection(int int_id)
{
    osg::Vec3 result_vec3;
    return result_vec3;
}


void PhysicsWorldFunctions::enableRealTimeCollisioinDetection()
{
    PhysicsWorld::instance()->setEnableRealTimeCollisioinDetection(true);
}

void PhysicsWorldFunctions::disableRealTimeCollisioinDetection()
{
    PhysicsWorld::instance()->setEnableRealTimeCollisioinDetection(false);
}

bool PhysicsWorldFunctions::getEnableRealTimeCollisioinDetection()
{
    return PhysicsWorld::instance()->getEnableRealTimeCollisioinDetection();
}

void PhysicsWorldFunctions::writeTransformToObjFileForOpenTissue( btCollisionObject *objWrapper)
{
    if(objWrapper != NULL)
    {
        btCollisionObject* collisionObj = (btCollisionObject*)objWrapper;
        const btRigidBody* rigidBody = dynamic_cast<btRigidBody*>(collisionObj);
        if(rigidBody != NULL)
        {
            btMotionState* btmotionState = (btMotionState*)rigidBody->getMotionState();
            osgbDynamics::MotionState* motionState = dynamic_cast<osgbDynamics::MotionState*>(btmotionState);
            if(motionState != NULL)
            {
                osg::Group* transformNode = motionState->getTransform();


                osg::ref_ptr<osg::MatrixTransform> newMT = new osg::MatrixTransform;
                for(int i = 0; i < transformNode->getNumChildren(); i++)
                {
                    Point3D* pointPtr = dynamic_cast<Point3D*>(transformNode->getChild(i));
                    if(pointPtr == NULL)
                    {
                        newMT->addChild(transformNode->getChild(i));
                    }

                }
                osg::Matrix m;
                //m.makeRotate(-1.57, 1,0,0);
                newMT->setMatrix(m);

                int ptrInt = (int)rigidBody;
                std::ostringstream out;
                out<<ptrInt;

                std::string fileName = out.str();
                fileName = "tempFiles/RigidBody/" + fileName + ".obj";
                osgDB::writeNodeFile(*newMT,fileName);
            }
        }
    }
}

void PhysicsWorldFunctions::writeRigidBodyInfoToXML()
{
    QDomDocument document;
    QDomElement docElement = document.createElement("Document");
    document.appendChild(docElement);
    btCollisionObjectArray objArray = PhysicsWorld::instance()->getDynamicsWorld()->getCollisionObjectArray();
    for(int i=0 ; i<objArray.size(); i++)
    {
        btCollisionObject* objPtr = objArray.at(i);
        QDomElement bodyElement = document.createElement("RigidBody");
        bodyElement.setAttribute("objName", QString::number((int)objPtr) + ".obj");
        docElement.appendChild(bodyElement);
        //std::cout<<"PhysicsWorldFunctions::writeRigidBodyInfoToXML objPtr="<<(int)objPtr<<std::endl;
        osg::Matrix matrix = osgbCollision::asOsgMatrix(objPtr->getWorldTransform());
        double x,y,z,angle;
        matrix.getRotate().getRotate(angle, x, y, z);
        QString quaStr = QString::number(angle) + ","
                +QString::number(x) + ","
                +QString::number(y) + ","
                +QString::number(z);
        QString transStr = QString::number(matrix.getTrans().x()) + ","+
                QString::number(matrix.getTrans().y()) + ","+
                QString::number(matrix.getTrans().z()) + ",";


        bodyElement.setAttribute("Matrix", transStr+quaStr);

        osg::Node* nodePtr = rigidBodyToOsgNode(dynamic_cast<btRigidBody*>(objPtr));
        if(nodePtr != NULL)
        {
            bodyElement.setAttribute("nodeName", QString::fromStdString(nodePtr->getName()) );
        }
        else
        {
            bodyElement.setAttribute("nodeName", QString("") );
        }

        writeTransformToObjFileForOpenTissue(objPtr);
    }

    QFile file("tempFiles/RigidBody/RigidBodyInfo.xml");
    file.open(QIODevice::WriteOnly);
    file.close();
    if(file.open(QIODevice::WriteOnly | QIODevice::Text) == false)
    {
        std::cout<<"save xml filed"<<std::endl;
    }
    QTextStream out(&file);
    out << document.toString();
    file.close();
}

void PhysicsWorldFunctions::writeZipOfRigidBody()
{
    //compress to the zip file
    JlCompress::compressDir("tempFiles/RigidBody.zip", "tempFiles/RigidBody");

}

void PhysicsWorldFunctions::sendZipTOSocket()
{
    QFile file("tempFiles/RigidBody/157347952.obj");
    if(file.open(QIODevice::ReadOnly | QIODevice::Text) == false)
    {
        std::cout<<"PhysicsWorldFunctions::sendZipTOSocket load zip filed"<<std::endl;
        return;
    }

    //QDataStream dataStream(&file);
    //QByteArray fileData;

    QTextStream in(&file);
    QString strContext = in.readAll();

    //std::cout<<strContext.toStdString()<<std::endl;


    QUdpSocket* sender = PhysicsWorld::instance()->getSenderSocket();
    QByteArray datagram = "hello world!";
    int length = sender->writeDatagram(strContext.toStdString().c_str(),QHostAddress::Broadcast, 65535);

    std::cout<<"PhysicsWorldFunctions::sendZipTOSocket:"<<length<<std::endl;

}
